% Opspace implemenation with DCA
clear

% All joints are revolute in this version
% The first body is attached to ground, the last body is free

% Fixed parameters for the system. Each body in the system is identical
NBODIES = 10;

MASS = 10;
INERTIA = 10;

% Center of mass located at the center of each link.
LENGTH = 1;

GRAVITY = [0 -9.81 0]';

% Initialize the state vector of the system
q(1:NBODIES,1) = -0.2; q(1) = 0;
u(1:NBODIES,1) = 0;

Zeta14 = zeros(6,NBODIES,NBODIES);
Zeta24 = zeros(6,NBODIES,NBODIES);

% For a revolute joint about positive z axis
% H is referred to as P^J_k in our paper. 
H = [0 0 1 0 0 0]';

% PREPROCESSING loop. Operations in this loop are performed just once and
% need not be timed.
for i=1:NBODIES
    
    % Initialize the fixed parameters of the system
    mass(i) = MASS;
    inertia(:,:,i) = [1 0 0;
                      0 1 0;
                      0 0 INERTIA];
                  
    % Spatial mass matrix written in body basis about the center of mass
    M(:,:,i) = [inertia(:,:,i) zeros(3,3);
                zeros(3,3) mass(i)*eye(3,3)];
    
    length(i) = LENGTH;
    
    % Vectors locating handles 1 and 2 with respect to center of mass
    % Written in body basis. 
    cmtoH1(:,i) = [-length(i)/2 0 0]';
    cmtoH2(:,i) = [length(i)/2 0 0]';
    
    % Written in body basis
    cmtoH1cross(:,:,i) = crossmat(cmtoH1(:,i)); 
                     
    cmtoH2cross(:,:,i) = crossmat(cmtoH2(:,i));
                     
    % Shift matrices for spatial constraint forces on handles H1 and H2
    SfH1(:,:,i) = [eye(3) cmtoH1cross(:,:,i);zeros(3) eye(3)];
    SfH2(:,:,i) = [eye(3) cmtoH2cross(:,:,i);zeros(3) eye(3)];
    
    % handle equations for each body are 
    % A_1 = zeta11 F1 + zeta12 F2 + zeta13
    % A_2 = zeta21 F1 + zeta22 F2 + zeta23
    % phi11, phi12, phi21 and phi22 are constant in body basis and computed
    % just once as a preprocessing step. 
    
    shift_inverse_H1(:,:,i) = (SfH1(:,:,i)')*inv(M(:,:,i));
    shift_inverse_H2(:,:,i) = (SfH2(:,:,i)')*inv(M(:,:,i));
    
    zeta11(:,:,i) = shift_inverse_H1(:,:,i)*SfH1(:,:,i); % (1a)
    zeta12(:,:,i) = shift_inverse_H1(:,:,i)*SfH2(:,:,i); % (1a)                
    
    zeta21(:,:,i) = shift_inverse_H2(:,:,i)*SfH1(:,:,i); % (1b)
    zeta22(:,:,i) = shift_inverse_H2(:,:,i)*SfH2(:,:,i); % (1b)   
    
    Zeta14(:,i,i) = shift_inverse_H1(:,:,i)*H;    
    Zeta24(:,i,i) = shift_inverse_H2(:,:,i)*H;
    if i~=NBODIES        
        Zeta14(:,i+1,i) = -shift_inverse_H1(:,:,i)*H;
        Zeta24(:,i+1,i) = -shift_inverse_H2(:,:,i)*H;
    end            
end

% Everything till this point was preprocessing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
param{1} = zeta11;
param{2} = zeta12;
param{3} = zeta21;
param{4} = zeta22;
param{5} = Zeta14;
param{6} = Zeta24;
param{7} = inertia;
param{8} = mass;
param{9} = GRAVITY;
param{10} = cmtoH1;
param{11} = cmtoH2;
param{12} = shift_inverse_H1;
param{13} = shift_inverse_H2;
param{14} = NBODIES;
% [ke,pe,e] = calcEnergy([q;u],length,param);
% udot = der(0,[q;u],param);
% keyboard
% draw(q,length);
% keyboard
options = odeset('RelTol',1.0e-7,'AbsTol',1E-7);
[t,x] = ode45(@der,[0 1],[q;u],options,param);
for i=1:max(size(t))
%     [ke, pe, e] = calcEnergy(x(i,:)',length,param);
%     E(i) = e;
%     PE(i) = sum(pe);
%     KE(i) = sum(ke);
    draw(x(i,1:NBODIES),length);
    pause
end